/**
 * @file sle_client.c
 * @brief SLE Client implementation for device discovery and connection management.
 *
 * This file contains the implementation of the SLE client, which is responsible for
 * discovering devices, managing connections, and handling communication with remote devices.
 *
 * @author Maker Studio
 * @license Apache 2.0
 *
 * This code is open-sourced under the Apache 2.0 license. When using open-source libraries
 * or code, please ensure to acknowledge the original authors and sources.
 */

#include "mpu6050.h"
#include "pinctrl.h"
#include "iot_i2c.h"
#include "iot_gpio.h"
#include "iot_gpio_ex.h"

#define MPU6050_ADDRESS 0x68
#define I2C_SET_BAUDRATE 400000
#define I2C_MASTER_ADDR 0x0
#define CONFIG_I2C_SCL_MASTER_PIN 15
#define CONFIG_I2C_SDA_MASTER_PIN 16
#define CONFIG_I2C_MASTER_PIN_MODE 2
#define CONFIG_I2C_MASTER_BUS_ID 1

#define MPU6050_PWR_MGMT_1 0x6B
#define MPU6050_PWR_MGMT_2 0x6C
#define MPU6050_SMPLRT_DIV 0x19
#define MPU6050_CONFIG 0x1A
#define MPU6050_GYRO_CONFIG 0x1B
#define MPU6050_ACCEL_CONFIG 0x1C
#define MPU6050_ACCEL_XOUT_H 0x3B
#define MPU6050_GYRO_XOUT_H 0x43

static void app_i2c_init_pin(void);
static void MPU6050_WriteReg(uint8_t reg, uint8_t data);
static uint8_t MPU6050_ReadReg(uint8_t reg);

/**
 * @brief Initialize the I2C pins.
 *
 * This function initializes the I2C pins for communication with the MPU6050 sensor.
 */
static void app_i2c_init_pin(void)
{
    IoTGpioInit(CONFIG_I2C_SCL_MASTER_PIN);
    IoTGpioInit(CONFIG_I2C_SDA_MASTER_PIN);
    IoSetFunc(CONFIG_I2C_SCL_MASTER_PIN, IOT_IO_FUNC_GPIO_15_I2C1_SDA);
    IoSetFunc(CONFIG_I2C_SDA_MASTER_PIN, IOT_IO_FUNC_GPIO_16_I2C1_SCL);
    IoSetPull(CONFIG_I2C_SDA_MASTER_PIN, PIN_PULL_TYPE_STRONG_UP);
    IoSetPull(CONFIG_I2C_SCL_MASTER_PIN, PIN_PULL_TYPE_STRONG_UP);
    IoTGpioSetDir(CONFIG_I2C_SCL_MASTER_PIN, IOT_GPIO_VALUE1);
    IoTGpioSetDir(CONFIG_I2C_SDA_MASTER_PIN, IOT_GPIO_VALUE1);
}

/**
 * @brief Write a value to a register on the MPU6050 sensor.
 *
 * This function writes a value to a specified register on the MPU6050 sensor.
 *
 * @param reg The register address.
 * @param data The value to write.
 */
static void MPU6050_WriteReg(uint8_t reg, uint8_t data)
{
    i2c_data_t mpudata = {0};
    uint8_t databuff[2] = {reg, data};
    mpudata.send_buf = databuff;
    mpudata.send_len = sizeof(databuff);
    uapi_i2c_master_write(CONFIG_I2C_MASTER_BUS_ID, MPU6050_ADDRESS, &mpudata);
}

/**
 * @brief Read a value from a register on the MPU6050 sensor.
 *
 * This function reads a value from a specified register on the MPU6050 sensor.
 *
 * @param reg The register address.
 * @return The value read from the register.
 */
static uint8_t MPU6050_ReadReg(uint8_t reg)
{
    i2c_data_t mpudata = {0};
    uint8_t data[2] = {0};
    mpudata.send_buf = &reg;
    mpudata.send_len = 1;
    mpudata.receive_buf = data;
    mpudata.receive_len = 1;
    uapi_i2c_master_writeread(CONFIG_I2C_MASTER_BUS_ID, MPU6050_ADDRESS, &mpudata);
    return data[0];
}

/**
 * @brief Initialize the MPU6050 sensor.
 *
 * This function initializes the MPU6050 sensor by configuring its registers.
 */
void MPU6050_Init(void)
{
    app_i2c_init_pin();
    IoTI2cInit(CONFIG_I2C_MASTER_BUS_ID, I2C_SET_BAUDRATE);
    MPU6050_WriteReg(MPU6050_PWR_MGMT_1, 0x01);
    MPU6050_WriteReg(MPU6050_PWR_MGMT_2, 0x00);
    MPU6050_WriteReg(MPU6050_SMPLRT_DIV, 0x09);
    MPU6050_WriteReg(MPU6050_CONFIG, 0x06);
    MPU6050_WriteReg(MPU6050_GYRO_CONFIG, 0x18);
    MPU6050_WriteReg(MPU6050_ACCEL_CONFIG, 0x18);
}

/**
 * @brief Get data from the MPU6050 sensor.
 *
 * This function reads accelerometer and gyroscope data from the MPU6050 sensor.
 *
 * @param data A pointer to the structure to store the sensor data.
 */
void MPU6050_GetData(MPU6050_Data_t *data)
{
    uint8_t buf[14];
    for (int i = 0; i < 14; i++)
    {
        buf[i] = MPU6050_ReadReg(MPU6050_ACCEL_XOUT_H + i);
    }
    data->accel.x = (int16_t)((buf[0] << 8) | buf[1]);
    data->accel.y = (int16_t)((buf[2] << 8) | buf[3]);
    data->accel.z = (int16_t)((buf[4] << 8) | buf[5]);
    data->gyro.x = (int16_t)((buf[8] << 8) | buf[9]);
    data->gyro.y = (int16_t)((buf[10] << 8) | buf[11]);
    data->gyro.z = (int16_t)((buf[12] << 8) | buf[13]);
}